#include "PositionControl.h"
#include "TrajectoryControl.h"

F32 Kp_Pos;
F32 ReferencePosition[3];
U8 CurrentWaypointIndex = 0;
U8 TimeBased = 0;

F32 WaypointsX[] = {1.0, 1.0, 2.0, 2.0, 3.0, 3.0, 2.0, 2.0, 1.0, 1.0, 0.0};
F32 WaypointsY[] = {0.0, -1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0};
F32 WaypointsG[] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

F32 centerX;
F32 centerY;
F32 centerG;
F32 timestamp;
U8 targetLeft;

void InitPositionControlVariables (void) {
	U8 i;

	for (i=0; i<3; i++) {
		ReferencePosition[i] = 0.0;
	}

	Kp_Pos = 1.0;

	centerX = WaypointsX[0]/2;
	centerY = WaypointsY[0]/2;
	centerG = WaypointsG[0]/2;

	timestamp = 0;
}

F32 absoluteValue(F32 num){
	return num>0?num:-num;
}

void GoToReferencePosition (void) {

	DirectKinematics();

	//calculate and amplify the errors
	ReferenceGlobalSpeed[X] = Kp_Pos * (ReferencePosition[X] - Position[X]);
	ReferenceGlobalSpeed[Y] = Kp_Pos * (ReferencePosition[Y] - Position[Y]);
	ReferenceGlobalSpeed[G] = Kp_Pos * (ReferencePosition[G] - Position[G]);

	InverseKinematics();


	RotationalSpeedControl(M1);
	RotationalSpeedControl(M2);
	RotationalSpeedControl(M3);
}

int GoToWaypoint (void){


	if(Position[X] > WaypointsX[CurrentWaypointIndex] - 0.02 
	&& Position[X] < WaypointsX[CurrentWaypointIndex] + 0.02
	&& Position[Y] > WaypointsY[CurrentWaypointIndex] - 0.02
	&& Position[Y] < WaypointsY[CurrentWaypointIndex] + 0.02
	&& Position[G] > WaypointsG[CurrentWaypointIndex] - (PI/16)
	&& Position[G] < WaypointsG[CurrentWaypointIndex] + (PI/16)){
		if(!TimeBased && CurrentWaypointIndex + 1 < sizeof(WaypointsX)/sizeof(WaypointsX[0])){
			CurrentWaypointIndex++;
			timestamp = 0;
			centerX = (WaypointsX[CurrentWaypointIndex] - Position[X])/2;
			centerY = (WaypointsY[CurrentWaypointIndex] - Position[Y])/2;
			if(CurrentWaypointIndex > 0 &&
				WaypointsX[CurrentWaypointIndex] == WaypointsX[CurrentWaypointIndex - 1] &&
				WaypointsY[CurrentWaypointIndex] == WaypointsY[CurrentWaypointIndex - 1]){
				TimeBased = 1;
				RestartTrajectory();
			}
		}else if(TimeBased){
		}else{
			return 1;
		}
	}

		if(!TimeBased){
			DirectKinematics();
			if(timestamp<2)timestamp += 0.02;
			//ReferenceGlobalSpeed[X] = (-absoluteValue(WaypointsX[CurrentWaypointIndex] - Position[X] - centerX) + centerX)/absoluteValue(centerX);
			//ReferenceGlobalSpeed[Y] = (-absoluteValue(WaypointsY[CurrentWaypointIndex] - Position[Y] - centerY) + centerY)/absoluteValue(centerY);
			//ReferenceGlobalSpeed[G] = 0;

			//ReferenceGlobalSpeed[X] = timestamp / 2 * Kp_Pos * (WaypointsX[CurrentWaypointIndex] - Position[X]);
			//ReferenceGlobalSpeed[Y] = timestamp / 2 * Kp_Pos * (WaypointsY[CurrentWaypointIndex] - Position[Y]);
			//ReferenceGlobalSpeed[G] = timestamp / 2 * Kp_Pos * (WaypointsG[CurrentWaypointIndex] - Position[G]);


			ReferenceGlobalSpeed[X] = timestamp / 2 * Kp_Pos * (WaypointsX[CurrentWaypointIndex] - Position[X]);
			ReferenceGlobalSpeed[Y] = timestamp / 2 * Kp_Pos * (WaypointsY[CurrentWaypointIndex] - Position[Y]);
			ReferenceGlobalSpeed[G] = timestamp / 2 * Kp_Pos * (WaypointsG[CurrentWaypointIndex] - Position[G]);

			InverseKinematics();

			RotationalSpeedControl(M1);
			RotationalSpeedControl(M2);
			RotationalSpeedControl(M3);
		}else{
		
			DirectKinematics();

			if(Trajectory2()){
				TimeBased = 0;
				CurrentWaypointIndex++;
			}

			InverseKinematics();

			RotationalSpeedControl(M1);
			RotationalSpeedControl(M2);
			RotationalSpeedControl(M3);
		}
		return 0;
}




